// MIT License

// Copyright (c) 2019 Erin Catto

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "box2d/b2_collision.h"
#include "box2d/b2_distance.h"

void b2WorldManifold::Initialize(const b2Manifold* manifold,
              const b2Transform& xfA, float radiusA,
              const b2Transform& xfB, float radiusB)
{
  if (manifold->pointCount == 0)
  {
    return;
  }

  switch (manifold->type)
  {
  case b2Manifold::e_circles:
    {
      normal.Set(1.0f, 0.0f);
      b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
      b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
      if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
      {
        normal = pointB - pointA;
        normal.Normalize();
      }

      b2Vec2 cA = pointA + radiusA * normal;
      b2Vec2 cB = pointB - radiusB * normal;
      points[0] = 0.5f * (cA + cB);
      separations[0] = b2Dot(cB - cA, normal);
    }
    break;

  case b2Manifold::e_faceA:
    {
      normal = b2Mul(xfA.q, manifold->localNormal);
      b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
      
      for (int32 i = 0; i < manifold->pointCount; ++i)
      {
        b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
        b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
        b2Vec2 cB = clipPoint - radiusB * normal;
        points[i] = 0.5f * (cA + cB);
        separations[i] = b2Dot(cB - cA, normal);
      }
    }
    break;

  case b2Manifold::e_faceB:
    {
      normal = b2Mul(xfB.q, manifold->localNormal);
      b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);

      for (int32 i = 0; i < manifold->pointCount; ++i)
      {
        b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
        b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
        b2Vec2 cA = clipPoint - radiusA * normal;
        points[i] = 0.5f * (cA + cB);
        separations[i] = b2Dot(cA - cB, normal);
      }

      // Ensure normal points from A to B.
      normal = -normal;
    }
    break;
  }
}

void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
            const b2Manifold* manifold1, const b2Manifold* manifold2)
{
  for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
  {
    state1[i] = b2_nullState;
    state2[i] = b2_nullState;
  }

  // Detect persists and removes.
  for (int32 i = 0; i < manifold1->pointCount; ++i)
  {
    b2ContactID id = manifold1->points[i].id;

    state1[i] = b2_removeState;

    for (int32 j = 0; j < manifold2->pointCount; ++j)
    {
      if (manifold2->points[j].id.key == id.key)
      {
        state1[i] = b2_persistState;
        break;
      }
    }
  }

  // Detect persists and adds.
  for (int32 i = 0; i < manifold2->pointCount; ++i)
  {
    b2ContactID id = manifold2->points[i].id;

    state2[i] = b2_addState;

    for (int32 j = 0; j < manifold1->pointCount; ++j)
    {
      if (manifold1->points[j].id.key == id.key)
      {
        state2[i] = b2_persistState;
        break;
      }
    }
  }
}

// From Real-time Collision Detection, p179.
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
{
  float tmin = -b2_maxFloat;
  float tmax = b2_maxFloat;

  b2Vec2 p = input.p1;
  b2Vec2 d = input.p2 - input.p1;
  b2Vec2 absD = b2Abs(d);

  b2Vec2 normal;

  for (int32 i = 0; i < 2; ++i)
  {
    if (absD(i) < b2_epsilon)
    {
      // Parallel.
      if (p(i) < lowerBound(i) || upperBound(i) < p(i))
      {
        return false;
      }
    }
    else
    {
      float inv_d = 1.0f / d(i);
      float t1 = (lowerBound(i) - p(i)) * inv_d;
      float t2 = (upperBound(i) - p(i)) * inv_d;

      // Sign of the normal vector.
      float s = -1.0f;

      if (t1 > t2)
      {
        b2Swap(t1, t2);
        s = 1.0f;
      }

      // Push the min up
      if (t1 > tmin)
      {
        normal.SetZero();
        normal(i) = s;
        tmin = t1;
      }

      // Pull the max down
      tmax = b2Min(tmax, t2);

      if (tmin > tmax)
      {
        return false;
      }
    }
  }

  // Does the ray start inside the box?
  // Does the ray intersect beyond the max fraction?
  if (tmin < 0.0f || input.maxFraction < tmin)
  {
    return false;
  }

  // Intersection.
  output->fraction = tmin;
  output->normal = normal;
  return true;
}

// Sutherland-Hodgman clipping.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
            const b2Vec2& normal, float offset, int32 vertexIndexA)
{
  // Start with no output points
  int32 count = 0;

  // Calculate the distance of end points to the line
  float distance0 = b2Dot(normal, vIn[0].v) - offset;
  float distance1 = b2Dot(normal, vIn[1].v) - offset;

  // If the points are behind the plane
  if (distance0 <= 0.0f) vOut[count++] = vIn[0];
  if (distance1 <= 0.0f) vOut[count++] = vIn[1];

  // If the points are on different sides of the plane
  if (distance0 * distance1 < 0.0f)
  {
    // Find intersection point of edge and plane
    float interp = distance0 / (distance0 - distance1);
    vOut[count].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);

    // VertexA is hitting edgeB.
    vOut[count].id.cf.indexA = static_cast<uint8>(vertexIndexA);
    vOut[count].id.cf.indexB = vIn[0].id.cf.indexB;
    vOut[count].id.cf.typeA = b2ContactFeature::e_vertex;
    vOut[count].id.cf.typeB = b2ContactFeature::e_face;
    ++count;

    b2Assert(count == 2);
  }

  return count;
}

bool b2TestOverlap(	const b2Shape* shapeA,
          const b2Shape* shapeB,
          const b2Transform& xfA, const b2Transform& xfB)
{
  b2DistanceInput input;
  input.proxyA.Set(shapeA);
  input.proxyB.Set(shapeB);
  input.transformA = xfA;
  input.transformB = xfB;
  input.useRadii = true;

  b2SimplexCache cache;
  cache.count = 0;

  b2DistanceOutput output;

  b2Distance(&output, &cache, &input);

  return output.distance < 10.0f * b2_epsilon;
}
